#include "./status_task.h"
#include "../cargo_driver/sensor.h"
#include "../cargo_driver/cargo_motor.h"
#include "../cargo_driver/pick_motor.h"
#include "../pub_driver/cmd_can.h"
#include "../utils/log.h"

namespace cargo_task {

using namespace cargo_driver;
using namespace pub_driver;
using namespace conn;

StatusTask statusTask;

static constexpr uint16_t VERSION = 0x12;

void StatusTask::run() {

    for (;;) {
        uint8_t sw2 = (pickMotor.is_alm()) |
        (cargoMotor->is_alm() << 1);

        cmdCAN->write<type::NOTIFY>(
            addr::IPC,
            proto::cargo::STATUS_UPLOAD,
            VERSION,
            sensor.read(),
            sw2,
            sensor.adc_value()
        );
        
        osDelay(500);
    }
}

}
